#include "coordMath.h"

NEDvector ECEFtoNED( ECEFvector coord, LLHvector home) {

    double cosLat = cos(home.lat);
    double sinLat = sin(home.lat);

    double cosLon = cos(home.lon);
    double sinLon = sin(home.lon);

    double conversionMatrix[3][3] = {
        {-sinLat * cosLon, -sinLat * sinLon, cosLat},
        {-sinLon,           cosLon,          0.0},
        {-cosLat * cosLon, -cosLat * sinLon, -sinLat}};

    NEDvector temp;

    temp.north = conversionMatrix[0][0] * coord.x + conversionMatrix[0][1] * coord.y + conversionMatrix[0][2] * coord.z;
    temp.east  = conversionMatrix[1][0] * coord.x + conversionMatrix[1][1] * coord.y + conversionMatrix[1][2] * coord.z;
    temp.down  = conversionMatrix[2][0] * coord.x + conversionMatrix[2][1] * coord.y + conversionMatrix[2][2] * coord.z;

    /* this is a kludge and differs from the code provided by rotomotion.
     * it apparently fixes the bug but it may be covering up an error somewhere else.
     */
    temp.down = -temp.down;
    
    return temp;
    
} /* end ECEFtoNED */
